/**
 * @file curvevoxel_cluster.cpp
 * @author tangjf (ryontang@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-03-11
 * 
 * @copyright Copyright (c) 2020
 * 
 */

// include personal lib.
//#include "curvevoxel_cluster.h"
#include "n_lidar_obj/depth_cluster/curvevoxel_cluster.h"

// include system lib.
#include <cmath>
#include <string>

namespace lidar
{
namespace obj
{

using namespace std;
const float PI = 3.1415926;

bool compare_cluster(pair<int, int> a, pair<int, int> b)
{
  return a.second > b.second;
} //升序

/**
 * @brief 计算y/x的反正切角度，角度按照右手定则，逆时针方向累加
 * 
 * @param x 
 * @param y 
 * @return float 
 */
float Curvevoxel_cluster::Polar_angle_cal(float x, float y)
{
  float temp_tangle = 0;
  if (x == 0 && y == 0)
  {
    temp_tangle = 0;
  }
  else if (y >= 0)
  {
    temp_tangle = (float)atan2(y, x);
  }
  else if (y <= 0)
  {
    temp_tangle = (float)atan2(y, x) + 2 * PI;
  }
  // 总之0=<temp_tangle<=2pi
  return temp_tangle;
}

// 比较关键
void Curvevoxel_cluster::build_hash_table(const vector<PointAPR> &vapr, unordered_map<int, Voxel> &map_out)
{
  vector<int> ri;
  vector<int> pi;
  vector<int> ai;
  // 遍历每一个点的弧坐标(pou,theta,phi)
  for (int i = 0; i < vapr.size(); ++i)
  {
    // 垂直角的分辨率是deltaA=2度 round(x)把x四舍五入
    int azimuth_index = round(((vapr[i].azimuth - minazimuth) * 180 / PI) / deltaA);
    int polar_index = round(vapr[i].polar_angle * 180 / PI / deltaP);
    int range_index = round((vapr[i].range - minrange) / deltaR);

    // voxel_index??? 通过hash函数得到hash值，每一个三维坐标与一个hash值对应
    //(polar_index,range_index, azimuth_index)->(voxel_index)
    int voxel_index = (polar_index * (length + 1) + range_index) + azimuth_index * (length + 1) * (width + 1);
    ri.push_back(range_index);
    pi.push_back(polar_index);
    ai.push_back(azimuth_index);
    unordered_map<int, Voxel>::iterator it_find;
    it_find = map_out.find(voxel_index);

    // 若map里面找到当前的voxel_index，把这个点云索引i存入对应map的迭代器it_find的second的index中
    if (it_find != map_out.end())
    {
      // 存在vapr中的第ith个点
      it_find->second.index.push_back(i);
    }
    else // map里面若没找到当前的voxel_index,那就添加
    {
      Voxel vox;
      vox.haspoint = true;
      vox.index.push_back(i);
      // (vector a).swap(veactor b)
      ////将 myvector 容量缩减至目前数据量的大小
      vox.index.swap(vox.index);
      map_out.insert(make_pair(voxel_index, vox));
    }
  }
  // 最大位置的索引
  auto maxPosition = max_element(ai.begin(), ai.end());
  auto maxPosition1 = max_element(ri.begin(), ri.end());
  auto maxPosition2 = max_element(pi.begin(), pi.end());
  cout << *maxPosition << " " << *maxPosition1 << " " << *maxPosition2 << endl;
}

void Curvevoxel_cluster::find_neighbors(int polar, int range, int azimuth, vector<int> &neighborindex)
{
  // azimuth三邻域
  for (int z = azimuth - 1; z <= azimuth + 1; z++)
  {
    // 越界
    if (z < 0 || z > round((maxazimuth - minazimuth) * 180 / PI / deltaA))
    {
      continue;
    }
    // range三邻域
    for (int y = range - 1; y <= range + 1; y++)
    {
      // 邻域的maxrange是50,阈值
      if (y < 0 || y > round((50 - minrange) / deltaR))
      {
        continue;
      }
      // polar三邻域
      for (int x = polar - 1; x <= polar + 1; x++)
      {
        int px = x;
        if (x < 0)
        {
          px = 300;
        }
        if (x > 300)
        {
          px = 0;
        }
        // int voxel_index = (polar_index * (length + 1) + range_index) + azimuth_index * (length + 1) * (width + 1);
        // 在polar_index/range_index/azimuth_index的[-1 0 1]中寻找
        neighborindex.push_back((px * (length + 1) + y) + z * (length + 1) * (width + 1));
      }
    }
  }
}

bool Curvevoxel_cluster::most_frequent_value(vector<int> values, vector<int> &cluster_index)
{
  unordered_map<int, int> histcounts;
  for (int i = 0; i < values.size(); i++)
  {
    // 若map里面没找到当前的values,都到最后了
    if (histcounts.find(values[i]) == histcounts.end())
    {
      // values[i]是聚类id histcounts<聚类id, 相同id的点数量>
      histcounts[values[i]] = 1;
    }
    else
    {
      histcounts[values[i]] += 1;
    }
  }

  int max = 0, maxi;
  vector<pair<int, int>> tr(histcounts.begin(), histcounts.end());
  // 将tr根据pair<聚类id, 点的数量>中点数量的从大到小排序
  sort(tr.begin(), tr.end(), compare_cluster);
  for (int i = 0; i < tr.size(); ++i)
  {
    // 点的个数大于10才算作一个类
    if (tr[i].second > 10)
    {
      // 筛选过后得到最终的cluster_index保存有id号
      cluster_index.push_back(tr[i].first);
    }
    else
    {
      cluster_index.push_back(-1);
    }
  }

  return true;
}

void Curvevoxel_cluster::mergeClusters(vector<int> &cluster_indices, int idx1, int idx2)
{
  for (int i = 0; i < cluster_indices.size(); i++)
  {
    // 对所有聚类编号进行更新，以邻居点的id来更新
    if (cluster_indices[i] == idx1)
    {
      cluster_indices[i] = idx2;
    }
  }
}

/**
 * @brief 聚类
 * 
 * @param map_in unordered_map<int, Voxel>中int是voxel_index每个voxel对应一个，Voxel:为点的索引
 * @param vapr 
 * @return vector<int> 
 */
vector<int> Curvevoxel_cluster::CVC(unordered_map<int, Voxel> &map_in, const vector<PointAPR> &vapr)
{
  int current_cluster = 0;
  cout << "CVC" << endl;
  // 初始化全为-1
  vector<int> cluster_indices = vector<int>(vapr.size(), -1);

  for (int i = 0; i < vapr.size(); ++i)
  {

    // 聚类的id号初始化全为-1，每一个点有一个聚类id:cluster_indices[i]
    if (cluster_indices[i] != -1)
      continue;
    // 等于-1往下执行求索引
    int azimuth_index = round((vapr[i].azimuth + fabs(minazimuth)) * 180 / PI / deltaA);
    int polar_index = round(vapr[i].polar_angle * 180 / PI / deltaP);
    int range_index = round((vapr[i].range - minrange) / deltaR);
    int voxel_index = (polar_index * (length + 1) + range_index) + azimuth_index * (length + 1) * (width + 1);

    unordered_map<int, Voxel>::iterator it_find;
    unordered_map<int, Voxel>::iterator it_find2;

    it_find = map_in.find(voxel_index);
    vector<int> neightbors;

    // 在map中找到voxel_index即某一个voxel
    if (it_find != map_in.end())
    {

      vector<int> neighborid;
      // 找该voxel对应的邻居
      find_neighbors(polar_index, range_index, azimuth_index, neighborid);
      for (int k = 0; k < neighborid.size(); ++k)
      {
        // 邻居点的voxel_index在hash表中吗
        it_find2 = map_in.find(neighborid[k]);
        // 仅当在的情况才是该voxel的邻居点
        if (it_find2 != map_in.end())
        {

          // 如果该邻居的voxel_index在map中找到，说明确实存在，取出里面所有点的索引
          for (int j = 0; j < it_find2->second.index.size(); ++j)
          {
            neightbors.push_back(it_find2->second.index[j]);
          }
        }
      }
    }

    // 当我们插入数据完成之后，想要释放多余的空间:vector<int>(ivec).swap(ivec),又叫做收缩到合适shrink to fit
    neightbors.swap(neightbors);

    if (neightbors.size() > 0)
    {
      // neightbors中装着所有同一个聚类的点的索引，把这些点的聚类编号都打上同一个标签
      for (int j = 0; j < neightbors.size(); ++j)
      {
        int oc = cluster_indices[i]; // 这是i！！！！本点的聚类id
        // neightbors[j]为某个序号voxel的index，即在vector<PointAPR> &vapr中实在的点的序号
        int nc = cluster_indices[neightbors[j]];
        if (oc != -1 && nc != -1)
        {
          if (oc != nc)
            mergeClusters(cluster_indices, oc, nc);
        }
        else
        {
          if (nc != -1)
          {
            cluster_indices[i] = nc;
          }
          else
          {
            if (oc != -1)
            {
              cluster_indices[neightbors[j]] = oc;
            }
          }
        }
      }
    }
    // cluster_indices[i]初始化
    if (cluster_indices[i] == -1)
    {
      // id从0开始打
      current_cluster++;
      cluster_indices[i] = current_cluster;
      for (int s = 0; s < neightbors.size(); ++s)
      {
        cluster_indices[neightbors[s]] = current_cluster;
      }
    }
  }
  return cluster_indices;
}

vector<float> Curvevoxel_cluster::hsv2rgb(vector<float> &hsv)
{
  vector<float> rgb(3);
  float R, G, B, H, S, V;
  H = hsv[0];
  S = hsv[1];
  V = hsv[2];
  if (S == 0)
  {
    rgb[0] = rgb[1] = rgb[2] = V;
  }
  else
  {

    int i = int(H * 6);
    float f = (H * 6) - i;
    float a = V * (1 - S);
    float b = V * (1 - S * f);
    float c = V * (1 - S * (1 - f));
    i = i % 6;
    switch (i)
    {
    case 0:
    {
      rgb[0] = V;
      rgb[1] = c;
      rgb[2] = a;
      break;
    }
    case 1:
    {
      rgb[0] = b;
      rgb[1] = V;
      rgb[2] = a;
      break;
    }
    case 2:
    {
      rgb[0] = a;
      rgb[1] = V;
      rgb[2] = c;
      break;
    }
    case 3:
    {
      rgb[0] = a;
      rgb[1] = b;
      rgb[2] = V;
      break;
    }
    case 4:
    {
      rgb[0] = c;
      rgb[1] = a;
      rgb[2] = V;
      break;
    }
    case 5:
    {
      rgb[0] = V;
      rgb[1] = a;
      rgb[2] = b;
      break;
    }
    }
  }

  return rgb;
}
} // namespace obj
} // namespace lidar